#pragma once

#include <driver/i2s.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <Arduino.h>
#define SERIAL_OUTPUT

//template<unsigned SAMPLE_COUNT, int PIN_WS = 18, int PIN_SCK = 23, int PIN_SD = 19, i2s_port_t I2S_PORT = I2S_NUM_0, , bool MSB_SHIFT = false, unsigned SAMPLE_RATE_HZ = 48000>

#define PIN_WS 33
#define PIN_SCK 32
#define PIN_SD 34
#define SAMPLE_RATE_HZ 16000  // Hz, fixed to design of IIR filters. Determines maximum frequency that can be analysed by the FFT Fmax=sampleF/2.
#define SAMPLE_COUNT 1024      // ~10ms sample time, must be power-of-two
#define I2S_PORT I2S_NUM_0
#define MIC_BITS 24
#define MSB_SHIFT false

#define MIC_EQUALIZER INMP441  // See below for defined IIR filters or set to 'None' to disable
#define MIC_OFFSET_DB 3.0103f  // Default offset (sine-wave RMS vs. dBFS). Modify this value for linear calibration
// Customize these values from microphone datasheet
#define MIC_SENSITIVITY -26.0f  // dBFS value expected at MIC_REF_DB (Sensitivity value from datasheet)
#define MIC_REF_DB 94.0f        // Value at which point sensitivity is specified in datasheet (dB)
#define MIC_OVERLOAD_DB 120.0f  // dB - Acoustic overload point / Maximum Acoustic Input
#define MIC_NOISE_DB 33.0f      // dB - Noise floor \
                                // valid number of bits in I2S data

class Microphone_I2S {
  static constexpr unsigned TASK_PRIO = 1;      // FreeRTOS priority
  static constexpr unsigned TASK_STACK = 2048;  // FreeRTOS stack size (in 32-bit words)

public:
  using SAMPLE_T = int32_t;
  using SampleBuffer = float[SAMPLE_COUNT];
  static const constexpr uint32_t SAMPLE_BITS = sizeof(SAMPLE_T) * 8;

  void begin() {
    Serial.print("installing microphone I2S driver at ");
    if (I2S_PORT == I2S_NUM_0) {
      Serial.println("I2S0");
    } else if (I2S_PORT == I2S_NUM_1) {
      Serial.println("I2S1");
    } else {
      Serial.println("unknown port");
    }

    i2s_config_t i2s_config{};
    i2s_config.mode = i2s_mode_t(I2S_MODE_MASTER | I2S_MODE_RX);
    i2s_config.sample_rate = SAMPLE_RATE_HZ;
    i2s_config.bits_per_sample = i2s_bits_per_sample_t(SAMPLE_BITS);
    i2s_config.channel_format = I2S_CHANNEL_FMT_ONLY_LEFT;
    i2s_config.communication_format = i2s_comm_format_t(I2S_COMM_FORMAT_STAND_I2S);
    i2s_config.intr_alloc_flags = ESP_INTR_FLAG_LEVEL1;
    i2s_config.dma_buf_count = 5;
    i2s_config.dma_buf_len = SAMPLE_COUNT;
    i2s_config.use_apll = true;
    i2s_config.fixed_mclk = 0;
    i2s_driver_install(I2S_PORT, &i2s_config, 0, nullptr);

    //Serial.println("Installing microphone I2S pin mapping");

    i2s_pin_config_t pin_config{};
    pin_config.bck_io_num = PIN_SCK;
    pin_config.ws_io_num = PIN_WS;
    pin_config.data_out_num = -1;  // not used
    pin_config.data_in_num = PIN_SD;
    if (MSB_SHIFT) {
      REG_SET_BIT(I2S_TIMING_REG(I2S_PORT), BIT(9));
      REG_SET_BIT(I2S_CONF_REG(I2S_PORT), I2S_RX_MSB_SHIFT);
    }
    i2s_set_pin(I2S_PORT, &pin_config);

    //Serial.println("creating microphone I2S sample queue");
    m_sampleQueue = xQueueCreate(2, sizeof(SampleBuffer));

    //Serial.println("creating microphone I2S reader task");
    xTaskCreate(readerTask, "Microphone_I2S reader", TASK_STACK, this, TASK_PRIO, nullptr);
  }

  QueueHandle_t sampleQueue() const {
    return m_sampleQueue;
  }

  void startSampling() {
    m_isSampling = true;
  }

  void stopSampling() {
    m_isSampling = false;
  }

private:

  QueueHandle_t m_sampleQueue;
  SampleBuffer m_sampleBuffer __attribute__((aligned(4)));
  volatile bool m_isSampling = false;

  static void readerTask(void *parameter) {
    Serial.println("Mic reader task started");
    auto object = reinterpret_cast<Microphone_I2S *>(parameter);
    size_t bytes_read = 0;
    for (int i = 0; i < 5; i++) {
      i2s_read(I2S_PORT, &object->m_sampleBuffer, SAMPLE_COUNT * sizeof(SAMPLE_T), &bytes_read, portMAX_DELAY);
    }

    while (true) {
      if (object->m_isSampling) {
        i2s_read(I2S_PORT, &object->m_sampleBuffer, SAMPLE_COUNT * sizeof(SAMPLE_T), &bytes_read, portMAX_DELAY);

        //i2s 32bit ->  24bit
        auto int_samples = reinterpret_cast<const SAMPLE_T *>(&object->m_sampleBuffer);
        for (int i = 0; i < SAMPLE_COUNT; i++) {
          object->m_sampleBuffer[i] = int_samples[i] >> (SAMPLE_BITS - MIC_BITS);
        }

        xQueueSend(object->m_sampleQueue, &object->m_sampleBuffer, portMAX_DELAY);

      } else {
        delay(10);
      }
    }
  }
};